#include "visualizer/splineVisualizer.h"

namespace lio {
  
	void splineVisualizer::bindTrajectory(TrajectoryManager::Ptr &traj_manager) {
		traj_manager_ = traj_manager;
		last_past_time = traj_manager_->getTrajectory()->R3Spline()->t0();
		delta = traj_manager_->getTrajectory()->R3Spline()->dt() / 10.0;

		double duration = 2000.0;
		past_path.poses.reserve(duration / delta);  // 预先申请 2000s 的数据

		setMarkerAttribute();

		past_markers.markers.reserve(duration / traj_manager_->getTrajectory()->R3Spline()->dt());
	}

	void splineVisualizer::showSplinePath() {
		updatePath();
		pubPath();
	}

	void splineVisualizer::updatePath() {
		past_path.poses.clear();
		while (last_past_time < traj_manager_->staticSeg.t0) {
			Eigen::Vector3d pos;
			Eigen::Quaterniond rot;
			traj_manager_->evaluateImuPose(last_past_time, pos, rot);
			geometry_msgs::PoseStamped pose;
			pose.pose.position.x = pos.x();
			pose.pose.position.y = pos.y();
			pose.pose.position.z = pos.z();
			past_path.poses.push_back(pose);

			last_past_time += delta;
		}

		double active_start_time = traj_manager_->staticSeg.t0;
		double active_end_time = traj_manager_->activeSeg.t0 + traj_manager_->activeSeg.dt * traj_manager_->activeSeg.n;
		active_path.poses.clear();

		double newest_start_time = traj_manager_->newestSeg.t0;
		double newest_end_time = traj_manager_->newestSeg.t0 + traj_manager_->newestSeg.dt * traj_manager_->newestSeg.n;
		newest_path.poses.clear();
		LOG(INFO) << "active_start_time: " << active_start_time - traj_manager_->getTrajectory()->R3Spline()->t0();
		LOG(INFO) << "active_end_time: " << active_end_time - traj_manager_->getTrajectory()->R3Spline()->t0();
		LOG(INFO) << "newest_start_time: " << newest_start_time - traj_manager_->getTrajectory()->R3Spline()->t0();
		LOG(INFO) << "newest_end_time: " << newest_end_time - traj_manager_->getTrajectory()->R3Spline()->t0();

		for (double ac = active_start_time; ac < newest_start_time; ac += delta) {
			Eigen::Vector3d pos;
			Eigen::Quaterniond rot;
			traj_manager_->evaluateImuPose(ac, pos, rot);
			geometry_msgs::PoseStamped pose;
			pose.pose.position.x = pos.x();
			pose.pose.position.y = pos.y();
			pose.pose.position.z = pos.z();
			// std::cout << "active time : " << ac - traj_manager_->getTrajectory()->R3Spline()->t0()
			// << "  pos: " << pos.transpose() << std::endl;
			active_path.poses.push_back(pose);
		}

		for (double ne = newest_start_time; ne < newest_end_time; ne += delta) {
			Eigen::Vector3d pos;
			Eigen::Quaterniond rot;
			traj_manager_->evaluateImuPose(ne, pos, rot);
			geometry_msgs::PoseStamped pose;
			pose.pose.position.x = pos.x();
			pose.pose.position.y = pos.y();
			pose.pose.position.z = pos.z();
			// std::cout << "newest time : " << ne - traj_manager_->getTrajectory()->R3Spline()->t0()
			// << "  pos: " << pos.transpose() << std::endl;
			newest_path.poses.push_back(pose);
		}

	}

	void splineVisualizer::pubPath() {
		past_path.header.stamp = ros::Time().fromSec(traj_manager_->staticSeg.t0);
		past_path_pub.publish(past_path);

		active_path.header.stamp = ros::Time().fromSec(traj_manager_->activeSeg.t0);
		active_path_pub.publish(active_path);

		newest_path.header.stamp = ros::Time().fromSec(traj_manager_->newestSeg.t0);
		newest_path_pub.publish(newest_path);
	}

	void splineVisualizer::setMarkerAttribute() {

		double point_scale = 0.1;
		past_marker.header.stamp = ros::Time();
		past_marker.ns = "past_marker";
		past_marker.type = visualization_msgs::Marker::SPHERE_LIST;
		past_marker.action = visualization_msgs::Marker::ADD;
		past_marker.id = 0;
		past_marker.pose.position.x = 0;
		past_marker.pose.position.y = 0;
		past_marker.pose.position.z = 0;
		past_marker.pose.orientation.x = 0.0;
		past_marker.pose.orientation.y = 0.0;
		past_marker.pose.orientation.z = 0.0;
		past_marker.pose.orientation.w = 1.0;
		past_marker.scale.x = point_scale;
		past_marker.scale.y = point_scale;
		past_marker.scale.z = point_scale;
		past_marker.color.a = 1.0; // Don't forget to set the alpha!
		past_marker.color.r = 1;
		past_marker.color.g = 1;
		past_marker.color.b = 1;

		static_marker.header.stamp = ros::Time();
		static_marker.ns = "static_marker";
		static_marker.type = visualization_msgs::Marker::SPHERE_LIST;
		static_marker.action = visualization_msgs::Marker::ADD;
		static_marker.id = 1;
		static_marker.pose.position.x = 0;
		static_marker.pose.position.y = 0;
		static_marker.pose.position.z = 0;
		static_marker.pose.orientation.x = 0.0;
		static_marker.pose.orientation.y = 0.0;
		static_marker.pose.orientation.z = 0.0;
		static_marker.pose.orientation.w = 1.0;
		static_marker.scale.x = point_scale;
		static_marker.scale.y = point_scale;
		static_marker.scale.z = point_scale;
		static_marker.color.a = 1.0; // Don't forget to set the alpha!
		static_marker.color.r = 0;
		static_marker.color.g = 1;
		static_marker.color.b = 0;

		active_marker.header.stamp = ros::Time();
		active_marker.ns = "active_marker";
		active_marker.type = visualization_msgs::Marker::SPHERE_LIST;
		active_marker.action = visualization_msgs::Marker::ADD;
		active_marker.id = 2;
		active_marker.pose.position.x = 0;
		active_marker.pose.position.y = 0;
		active_marker.pose.position.z = 0;
		active_marker.pose.orientation.x = 0.0;
		active_marker.pose.orientation.y = 0.0;
		active_marker.pose.orientation.z = 0.0;
		active_marker.pose.orientation.w = 1.0;
		active_marker.scale.x = point_scale;
		active_marker.scale.y = point_scale;
		active_marker.scale.z = point_scale;
		active_marker.color.a = 1.0; // Don't forget to set the alpha!
		active_marker.color.r = 0;
		active_marker.color.g = 0;
		active_marker.color.b = 1;

		newest_marker.header.stamp = ros::Time();
		newest_marker.ns = "newest_marker";
		newest_marker.type = visualization_msgs::Marker::SPHERE_LIST;
		newest_marker.action = visualization_msgs::Marker::ADD;
		newest_marker.id = 3;
		newest_marker.pose.position.x = 0;
		newest_marker.pose.position.y = 0;
		newest_marker.pose.position.z = 0;
		newest_marker.pose.orientation.x = 0.0;
		newest_marker.pose.orientation.y = 0.0;
		newest_marker.pose.orientation.z = 0.0;
		newest_marker.pose.orientation.w = 1.0;
		newest_marker.scale.x = point_scale;
		newest_marker.scale.y = point_scale;
		newest_marker.scale.z = point_scale;
		newest_marker.color.a = 1.0; // Don't forget to set the alpha!
		newest_marker.color.r = 1;
		newest_marker.color.g = 0;
		newest_marker.color.b = 0;
	}

	void splineVisualizer::showSplineControl() {
		updateMarkerArray();
		pubMarkerArray();
	}

	void splineVisualizer::updateMarkerArray() {
		int static_fid = traj_manager_->staticSeg.fid;
		int static_n = traj_manager_->staticSeg.n;

		int active_fid = traj_manager_->activeSeg.fid;
		int active_n = traj_manager_->activeSeg.n;

		int newest_fid = traj_manager_->newestSeg.fid;
		int newest_n = traj_manager_->newestSeg.n;

		past_marker.points.clear();
		while (past_marker.points.size() < static_fid) {
			int id = past_marker.points.size();
			Eigen::Vector3d cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(id);
			past_marker.points.emplace_back();
			past_marker.points.back().x = cp.x();
			past_marker.points.back().y = cp.y();
			past_marker.points.back().z = cp.z();
		}
		past_markers.markers.clear();
		past_markers.markers.push_back(past_marker);

		// auto bef_static_markers = static_markers.markers;
		// static_markers.markers.clear();
		// for (auto &mark : bef_static_markers){
		// 	mark.action = visualization_msgs::Marker::DELETE;
		// 	static_markers.markers.push_back(mark);
		// }
		static_marker.points.clear();
		for (int i = static_fid; i < static_fid + 3; ++i) {
			Eigen::Vector3d cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(i);
			static_marker.points.emplace_back();
			static_marker.points.back().x = cp.x();
			static_marker.points.back().y = cp.y();
			static_marker.points.back().z = cp.z();
		}
		static_markers.markers.clear();
		static_markers.markers.push_back(static_marker);

		// auto bef_active_markers = active_markers.markers;
		// active_markers.markers.clear();
		// for (auto &mark : bef_active_markers){
		// 	mark.action = visualization_msgs::Marker::DELETE;
		// 	active_markers.markers.push_back(mark);
		// }
		active_marker.points.clear();
		for (int i = active_fid; i < newest_fid + 3; ++i) {
			Eigen::Vector3d cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(i);
			active_marker.points.emplace_back();
			active_marker.points.back().x = cp.x();
			active_marker.points.back().y = cp.y();
			active_marker.points.back().z = cp.z();
		}
		active_markers.markers.clear();
		active_markers.markers.push_back(active_marker);

		// auto bef_newest_markers = newest_markers.markers;
		// newest_markers.markers.clear();
		// for (auto &mark : bef_newest_markers){
		// 	mark.action = visualization_msgs::Marker::DELETE;
		// 	newest_markers.markers.push_back(mark);
		// }
		newest_marker.points.clear();
		for (int i = newest_fid + 3; i < newest_fid + newest_n + 3; ++i) {
			Eigen::Vector3d cp = traj_manager_->getTrajectory()->R3Spline()->GetContralPointaAt(i);
			newest_marker.points.emplace_back();
			newest_marker.points.back().x = cp.x();
			newest_marker.points.back().y = cp.y();
			newest_marker.points.back().z = cp.z();
		}
		newest_markers.markers.clear();
		newest_markers.markers.push_back(newest_marker);

	}

	void splineVisualizer::pubMarkerArray() {
		past_control_pub.publish(past_markers);
		static_control_pub.publish(static_markers);
		active_control_pub.publish(active_markers);
		newest_control_pub.publish(newest_markers);
	}

	void splineVisualizer::pubDiscretePath(std::vector<Eigen::Vector3d> &postions) {
		discrete_path.poses.clear();
		for (auto &pos : postions) {
			geometry_msgs::PoseStamped pose;
			pose.pose.position.x = pos.x();
			pose.pose.position.y = pos.y();
			pose.pose.position.z = pos.z();
			discrete_path.poses.push_back(pose);
		}
		discrete_path.header.stamp = ros::Time::now();
		discrete_path_pub.publish(discrete_path);
	}

}